#include "showApiBulidMap.h"


c_showApiBulidMap::c_showApiBulidMap(/* args */)
{

    
}

c_showApiBulidMap::~c_showApiBulidMap()
{
}


ros::Publisher *p_bulid_path_pub;
nav_msgs::Path bulidMap_path;


void c_showApiBulidMap::pubMesage_ros(float x, float y)
{
    geometry_msgs::PoseStamped this_pose_stamped;
    this_pose_stamped.pose.position.x = x;
    this_pose_stamped.pose.position.y = y;

    geometry_msgs::Quaternion goal_quat = tf::createQuaternionMsgFromYaw(0);
    this_pose_stamped.pose.orientation.x = goal_quat.x;
    this_pose_stamped.pose.orientation.y = goal_quat.y;
    this_pose_stamped.pose.orientation.z = goal_quat.z;
    this_pose_stamped.pose.orientation.w = goal_quat.w;

    this_pose_stamped.header.stamp=ros::Time::now();
    this_pose_stamped.header.frame_id="odom";
    bulidMap_path.poses.push_back(this_pose_stamped);

    p_bulid_path_pub->publish(bulidMap_path);
    ros::spinOnce();               // check for incoming messages
}



void c_showApiBulidMap::bulidMapCallback(const MowerProtoPakg::bulidMapLocalization& msg) 
{
    printf("bulidMap: %lf  %lf %lf  %lf\r\n", msg.wordx(), msg.wordy(), msg.startx(), msg.starty());
    pubMesage_ros(msg.wordx(), msg.wordy());
}


void c_showApiBulidMap::loop(void)
{
    ros::NodeHandle ph;
    ros::Publisher path_pub = ph.advertise<nav_msgs::Path>("bulidMap",1, true);
    p_bulid_path_pub = &path_pub;
    bulidMap_path.header.stamp = ros::Time::now();
    bulidMap_path.header.frame_id="odom";

    // recv protobuf of mower_mv    
    eCAL::protobuf::CSubscriber<MowerProtoPakg::bulidMapLocalization> subscriber("bulidMapMessage"); // ! 
    subscriber.AddReceiveCallback(std::bind(bulidMapCallback, std::placeholders::_2)); 
    while (eCAL::Ok()) {
        sleep(10);
    }
    eCAL::Finalize();// finalize eCAL API
}

void c_showApiBulidMap::start_thread(void)
{
    thread = std::thread(&c_showApiBulidMap::loop, this);
}
